
#include "../lettershell/shell_cpp.h"
#include "softbus.h"
#include <stdio.h>

namespace ShellCmd
{
    int GyroDriftCalibrate(int argc, char **argv)
    {
        static char nameBuf[64];
        float gyroStaticDrift[3] = {0.0f, 0.0f, 0.0f};

        if (argc == 2)
        {
            printf("Calibrating gyro drift for IMU \"%s\"!\n", argv[2]);
            snprintf(nameBuf, sizeof(nameBuf), "/%s/gyro-calibrate", argv[2]);
        }
        else
        {
            printf("Calibrating gyro drift for default IMU!\n");
            snprintf(nameBuf, sizeof(nameBuf), "/ins/gyro-calibrate");
        }

        auto ret = Bus_RemoteFuncCall(nameBuf, {{"ret", {&gyroStaticDrift}}});
        if (ret)
        {
            printf("Gyro static drift parameters:\n  %f\n  %f\n  %f\n",
                   gyroStaticDrift[0],
                   gyroStaticDrift[1],
                   gyroStaticDrift[2]);
        }
        else
        {
            printf("Gyro static drift calibration failed\n");
        }

        return !ret;
    }
} // namespace ShellCmd

// clang-format off
SHELL_EXPORT_CMD(SHELL_CMD_TYPE(SHELL_TYPE_CMD_MAIN), gyrocalibrate, ShellCmd::GyroDriftCalibrate, Calibrate IMU gyroscope static drift);